#include <ros/ros.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <linux/input.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
#include <tecbot_msgs/obstacle.h>
#include <common/common_state_code.h>
#include <navigation/navigation_state_code.h>
#include <localization/localization_sources.h>

#define NODE_NAME "obstacle_handler"
#define TAG "障碍检测"

using namespace std;

// 定义雷达视角参数
float laser_scan_forward_min_angle = -70.4 * M_PI / 180.0;
float laser_scan_forward_max_angle = 70.4 * M_PI / 180.0;
float laser_scan_backward_min_angle = 109.6 * M_PI / 180.0;
float laser_scan_backward_max_angle = -109.6 * M_PI / 180.0;

// 定义雷达距离参数
float laser_min_dist = 0.06;
float laser_max_dist = 20;

// 定义障碍数据清除超时
int obstalce_no_update_clear_timeout = 5;

// 定义底盘尺寸
float chassis_width = 0.85;
float chassis_length = 0.85;

// 定义网格数
int obstacle_grid_number = 5;

// 定义机器人雷达数据
sensor_msgs::LaserScan laser_scan_data;
ros::Time collect_start_time;

// 定义是否使用超声波
bool use_sonar = false;

// 定义超声波距离数据
float sonar_front_dist = 0;
float sonar_rear_dist = 0;
float sonar_left_dist = 0;
float sonar_right_dist = 0;

// 定义obstacle发布器
ros::Publisher obstacle_pub;

// 定义obstacle数据
tecbot_msgs::obstacle obstacle_data;

// 是否获取到雷达数据标记
bool laser_scan_data_ready = false;
bool lidar_data_ready = false;
bool sonar_front_data_ready = false;
bool sonar_rear_data_ready = false;
bool sonar_left_data_ready = false;
bool sonar_right_data_ready = false;

// laser_scan回调函数
void laser_scan_callback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
	laser_scan_data = *msg;
	laser_scan_data_ready = true;
}

// sonar_front 回调函数
void sonar_front_callback(const sensor_msgs::Range::ConstPtr &msg)
{
	sonar_front_dist = msg->range;
	sonar_front_data_ready = true;
}

// sonar_rear 回调函数
void sonar_rear_callback(const sensor_msgs::Range::ConstPtr &msg)
{
	sonar_rear_dist = msg->range;
	sonar_rear_data_ready = true;
}

// sonar_left 回调函数
void sonar_left_callback(const sensor_msgs::Range::ConstPtr &msg)
{
	sonar_left_dist = msg->range;
	sonar_left_data_ready = true;
}

// sonar_right 回调函数
void sonar_right_callback(const sensor_msgs::Range::ConstPtr &msg)
{
	sonar_right_dist = msg->range;
	sonar_right_data_ready = true;
}

int main(int argc, char **argv)
{
	// 初始化ROS节点
	ros::init(argc, argv, NODE_NAME);

	// 设置控制台本地化，以显示中文
	setlocale(LC_CTYPE, "zh_CN.utf8");

	// 打印调试信息
	ROS_INFO("%s：节点已启动。", TAG);

	// 定义ROS节点处理程序
	ros::NodeHandle node_handle;

	// 定义ROS节点循环频率控制器
	ros::Rate loop_rate(10);

	// 定义ROS参数获取程序
	ros::NodeHandle node_handle_param("~/obstacle");

	// 获取参数
	node_handle_param.param<float>("laser_scan_forward_min_angle", laser_scan_forward_min_angle, -90 * M_PI / 180.0);
	node_handle_param.param<float>("laser_scan_forward_max_angle", laser_scan_forward_max_angle, 90 * M_PI / 180.0);
	node_handle_param.param<float>("laser_scan_backward_min_angle", laser_scan_backward_min_angle, 109.6 * M_PI / 180.0);
	node_handle_param.param<float>("laser_scan_backward_max_angle", laser_scan_backward_max_angle, -109.6 * M_PI / 180.0);

	node_handle_param.param<float>("laser_min_dist", laser_min_dist, 0.06);
	node_handle_param.param<float>("laser_max_dist", laser_max_dist, 20);

	node_handle_param.param<int>("obstalce_no_update_clear_timeout", obstalce_no_update_clear_timeout, 20);

	node_handle_param.param<float>("chassis_width", chassis_width, 0.80);
	node_handle_param.param<float>("chassis_length", chassis_length, 0.8262);

	node_handle_param.param<int>("obstacle_grid_number", obstacle_grid_number, 5);

	node_handle_param.param<bool>("use_sonar", use_sonar, false);

	// 定义单个网格尺寸
	float obstacle_grid_size = chassis_width / obstacle_grid_number;

	// 雷达订阅器
	ros::Subscriber laser_scan_sub = node_handle.subscribe("tecbot/hardware/lidar/scan/obstacle", 2, laser_scan_callback);

	// 超声波订阅器
	ros::Subscriber sonar_front_sub = node_handle.subscribe("tecbot/hardware/sonar/front", 2, sonar_front_callback);
	ros::Subscriber sonar_rear_sub = node_handle.subscribe("tecbot/hardware/sonar/rear", 2, sonar_rear_callback);
	ros::Subscriber sonar_left_sub = node_handle.subscribe("tecbot/hardware/sonar/left", 2, sonar_left_callback);
	ros::Subscriber sonar_right_sub = node_handle.subscribe("tecbot/hardware/sonar/right", 2, sonar_right_callback);

	// obstacle 话题发布器
	obstacle_pub = node_handle.advertise<tecbot_msgs::obstacle>("tecbot/navigation/obstacle", 1);

	// 初始化obstacle数据
	obstacle_data.header.frame_id = LOCALIZATION_BASE_LINK;
	obstacle_data.size = obstacle_grid_number;

	// 初始化雷达数据
	std::vector<float> obstacle_front_data(obstacle_grid_number, INFINITY);
	std::vector<float> obstacle_rear_data(obstacle_grid_number, INFINITY);
	std::vector<float> obstacle_left_data(obstacle_grid_number, INFINITY);
	std::vector<float> obstacle_right_data(obstacle_grid_number, INFINITY);
	obstacle_data.obstacle_front = obstacle_front_data;
	obstacle_data.obstacle_rear = obstacle_rear_data;
	obstacle_data.obstacle_left = obstacle_left_data;
	obstacle_data.obstacle_right = obstacle_right_data;

	std::vector<int> obstacle_front_update_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_rear_update_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_left_update_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_right_update_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_front_noupdate_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_rear_noupdate_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_left_noupdate_counter(obstacle_grid_number, 0);
	std::vector<int> obstacle_right_noupdate_counter(obstacle_grid_number, 0);

	std::vector<int> obstacle_front_current_data_init(obstacle_grid_number, 0);
	std::vector<int> obstacle_rear_current_data_init(obstacle_grid_number, 0);
	std::vector<int> obstacle_left_current_data_init(obstacle_grid_number, 0);
	std::vector<int> obstacle_right_current_data_init(obstacle_grid_number, 0);

	// 初始化计时器
	collect_start_time = ros::Time::now();

	while (node_handle.ok())
	{
		// 注册回调
		ros::spinOnce();

		// 初始化obstacle数据
		obstacle_data.header.stamp = ros::Time::now();

		if (laser_scan_data_ready)
		{
			// 处理雷达前部数据
			int laser_scan_forward_min_index = (laser_scan_forward_min_angle - laser_scan_data.angle_min) / laser_scan_data.angle_increment;
			int laser_scan_forward_max_index = (laser_scan_forward_max_angle - laser_scan_data.angle_min) / laser_scan_data.angle_increment;
			int laser_scan_index = laser_scan_forward_min_index;

			// 处理雷达前部
			while (laser_scan_index != laser_scan_forward_max_index)
			{
				if (laser_scan_data.ranges[laser_scan_index] <= laser_scan_data.range_max &&
					laser_scan_data.ranges[laser_scan_index] >= laser_scan_data.range_min)
				{
					float current_angle = laser_scan_data.angle_min + laser_scan_index * laser_scan_data.angle_increment;
					float y_pos = laser_scan_data.ranges[laser_scan_index] * sin(current_angle);
					float x_pos = laser_scan_data.ranges[laser_scan_index] * cos(current_angle);

					if (abs(y_pos) <= chassis_width / 2 && x_pos >= laser_min_dist && x_pos <= laser_max_dist)
					{
						// 4 3 2 1 0
						// 计算位于哪个网格中
						int grid_index = (int)(y_pos / obstacle_grid_size) + (obstacle_grid_number / 2);
						grid_index = grid_index > obstacle_grid_number - 1 ? obstacle_grid_number - 1 : (grid_index < 0 ? 0 : grid_index);

						// 更新网格数据
						if (!obstacle_front_current_data_init[grid_index])
						{
							obstacle_front_current_data_init[grid_index] = 1;
							obstacle_front_data[grid_index] = x_pos;
						}
						else
						{
							obstacle_front_data[grid_index] = min(obstacle_front_data[grid_index], x_pos);
						}

						obstacle_front_noupdate_counter[grid_index] = 0;
					}
				}

				laser_scan_index++;
			}
		}

		if (use_sonar)
		{
			// 处理前方超声波数据
			if (sonar_front_data_ready)
			{
				int grid_index = obstacle_grid_number / 2;

				obstacle_front_data[grid_index] = min(obstacle_front_data[grid_index], sonar_front_dist);
			}

			// 处理后侧超声波数据
			if (sonar_rear_data_ready)
			{
				int grid_index = obstacle_grid_number / 2;

				obstacle_rear_data[obstacle_grid_number / 2] = sonar_rear_dist;

				obstacle_rear_noupdate_counter[grid_index] = 0;
			}

			// 处理左侧超声波数据
			if (sonar_left_data_ready)
			{
				int grid_index = obstacle_grid_number / 2;

				obstacle_left_data[obstacle_grid_number / 2] = sonar_left_dist;

				obstacle_left_noupdate_counter[grid_index] = 0;
			}

			// 处理右侧超声波数据
			if (sonar_right_data_ready)
			{
				int grid_index = obstacle_grid_number / 2;

				obstacle_right_data[obstacle_grid_number / 2] = sonar_right_dist;

				obstacle_right_noupdate_counter[grid_index] = 0;
			}
		}

		// 处理雷达障碍物数据强制更新
		for (int i = 0; i < obstacle_grid_number; i++)
		{
			obstacle_front_noupdate_counter[i]++;
			obstacle_rear_noupdate_counter[i]++;
			obstacle_left_noupdate_counter[i]++;
			obstacle_right_noupdate_counter[i]++;

			obstacle_front_current_data_init[i] = 0;
			obstacle_rear_current_data_init[i] = 0;
			obstacle_left_current_data_init[i] = 0;
			obstacle_right_current_data_init[i] = 0;

			if (obstacle_front_noupdate_counter[i] > obstalce_no_update_clear_timeout)
			{
				obstacle_front_noupdate_counter[i] = 0;
				obstacle_front_data[i] = INFINITY;
			}

			if (obstacle_rear_noupdate_counter[i] > obstalce_no_update_clear_timeout)
			{
				obstacle_rear_noupdate_counter[i] = 0;
				obstacle_rear_data[i] = INFINITY;
			}

			if (obstacle_left_noupdate_counter[i] > obstalce_no_update_clear_timeout)
			{
				obstacle_left_noupdate_counter[i] = 0;
				obstacle_left_data[i] = INFINITY;
			}

			if (obstacle_right_noupdate_counter[i] > obstalce_no_update_clear_timeout)
			{
				obstacle_right_noupdate_counter[i] = 0;
				obstacle_right_data[i] = INFINITY;
			}
		}

		// 更新障碍物数据
		for (int i = 0; i < obstacle_grid_number; i++)
		{
			// 更新前方障碍物
			if (obstacle_front_data[i] <= obstacle_data.obstacle_front[i])
			{
				obstacle_data.obstacle_front[i] = obstacle_front_data[i];
			}
			else if (obstacle_front_data[i] < INFINITY)
			{
				obstacle_front_update_counter[i]++;

				if (obstacle_front_update_counter[i] >= obstalce_no_update_clear_timeout)
				{
					obstacle_data.obstacle_front[i] = obstacle_front_data[i];
					obstacle_front_update_counter[i] = 0;
				}
			}
			else
			{
				obstacle_data.obstacle_front[i] = obstacle_front_data[i];
			}

			// 更新后方障碍物
			if (obstacle_rear_data[i] <= obstacle_data.obstacle_rear[i])
			{
				obstacle_data.obstacle_rear[i] = obstacle_rear_data[i];
			}
			else if (obstacle_rear_data[i] < INFINITY)
			{
				obstacle_rear_update_counter[i]++;

				if (obstacle_rear_update_counter[i] >= obstalce_no_update_clear_timeout)
				{
					obstacle_data.obstacle_rear[i] = obstacle_rear_data[i];
					obstacle_rear_update_counter[i] = 0;
				}
			}
			else
			{
				obstacle_data.obstacle_rear[i] = obstacle_rear_data[i];
			}

			// 更新左侧障碍物
			if (obstacle_left_data[i] <= obstacle_data.obstacle_left[i])
			{
				obstacle_data.obstacle_left[i] = obstacle_left_data[i];
			}
			else if (obstacle_left_data[i] < INFINITY)
			{
				obstacle_left_update_counter[i]++;

				if (obstacle_left_update_counter[i] >= obstalce_no_update_clear_timeout)
				{
					obstacle_data.obstacle_left[i] = obstacle_left_data[i];
					obstacle_left_update_counter[i] = 0;
				}
			}
			else
			{
				obstacle_data.obstacle_left[i] = obstacle_left_data[i];
			}

			// 更新右侧障碍物
			if (obstacle_right_data[i] <= obstacle_data.obstacle_right[i])
			{
				obstacle_data.obstacle_right[i] = obstacle_right_data[i];
			}
			else if (obstacle_right_data[i] < INFINITY)
			{
				obstacle_right_update_counter[i]++;

				if (obstacle_right_update_counter[i] >= obstalce_no_update_clear_timeout)
				{
					obstacle_data.obstacle_right[i] = obstacle_right_data[i];
					obstacle_right_update_counter[i] = 0;
				}
			}
			else
			{
				obstacle_data.obstacle_right[i] = obstacle_right_data[i];
			}
		}

		// 发布obstacle数据
		obstacle_pub.publish(obstacle_data);

		// 循环频率控制
		loop_rate.sleep();
	}

	return 0;
}